from array import *
from machine import *
import math
from collections import *



ccd2_left_idx_buxian  = 0
ccd2_right_idx_buxian = 0
image_value1 = [0] *128
image_value2 = [0] *128



#我的原创圆环


x = 0 # 状态标志位
fflag = 0 # 检测标志位
gyro = 0 # 辅助检测出环，z轴角速度
dist = 0 # 行驶距离 

lose_r_flag = 0 # 在出环时，ccd要丢线一次才行

def ave_threshold(ccd_data):
    '''
    求图像的平均阈值。
    
    参数：
        ccd_data： ccd图像数据 
    
    return：
        ave_thres： 平均阈值
    '''
    
    ave_thres = sum(ccd_data) //128
    
    return ave_thres

def maxmin_threshold(ccd_data):
    value_max = ccd_data[4]          # 从第5个元素开始考虑最大值
    value_min = ccd_data[4]          # 从第5个元素开始考虑最小值
    
    #遍历5-122
    for i in range(5, 123):
        value_max = max(value_max, ccd_data[i])   # 限幅在最大传入数据和第5个元素值上
        value_min = min(value_min, ccd_data[i])   # 限幅在最小传入数据和第5个元素值上
        
    threshold = (value_max + value_min) / 2      # 计算阈值
    threshold = min(max(75, threshold), 3900)     # 阈值限幅在75-256之间
    return threshold

def ccd_image_value(ccd_data, value):
    ccd_value = [0] * len(ccd_data)   # 定义一个空二值化列表
    for i in range(len(ccd_data)):    # 遍历0-127点
        temp_num = ccd_data[i]        # 暂存列表
        if temp_num > value:          # 比较阈值大小
            ccd_value[i] = 1          # 大于阈值为1
        else:
            ccd_value[i] = 0          # 小于为0
    return ccd_value        
    
#ccd滤波
def ccd_filter(ccd_n):
    global image_value1,image_value2
    if ccd_n == 1:  # 对CCD1采集的图像进行滤波  
        for i in range(1, 126):      # 防止越界
            if image_value1[i] == 1 and image_value1[i - 1] == 0 and image_value1[i + 1] == 0:  
                image_value1[i] = 0  # 如果当前为1，前后都为0，则将当前设置为0  
            elif image_value1[i] == 0 and image_value1[i - 1] == 1 and image_value1[i + 1] == 1:  
                image_value1[i] = 1  # 如果当前为0，前后都为1，则将当前设置为1  
  
    elif ccd_n == 2:  # 对CCD2采集的图像进行滤波  
        for i in range(1, 126):      # 防止越界
            if image_value2[i] == 1 and image_value2[i - 1] == 0 and image_value2[i + 1] == 0:  
                image_value2[i] = 0  # 如果当前为1，前后都为0，则将当前设置为0  
            elif image_value2[i] == 0 and image_value2[i - 1] == 1 and image_value2[i + 1] == 1:  
                image_value2[i] = 1  # 如果当前为0，前后都为1，则将当前设置为1  
    
    
# 得到中点
left_point_1 = False
right_point_1 = False
last_mid_point_1 = 0  # 上次中点，应在函数外部初始化  
mid_point_1 = 60      # 当前中点，应在函数外部初始化
def get_ccd1_mid_point(bin_ccd):  
    global last_mid_point_1, mid_point_1, left_point_1, right_point_1

    # 搜索左边点，以上次中点作为这次的起搜点  
    for l_point1 in range(last_mid_point_1, 1, -1):  
        if bin_ccd[l_point1 - 1] == 0 and bin_ccd[l_point1] == 1:    # 判断是否与左边点一致
            left_point_1 = l_point1     # 左边点找到
            break  
        elif l_point1 == 1:     # 如果找到1都没找到
            left_point_1 = 0    # 强制令左边点为0
            break  

      
    # 搜索右边点，以上次中点作为这次的起搜点  
    for r_point1 in range(last_mid_point_1, 126):  # 注意这里应该是128，因为索引是从0开始的  
        if bin_ccd[r_point1] == 1 and bin_ccd[r_point1 + 1] == 0:    # 判断是否与右边点一致
            right_point_1 = r_point1    # 右边点找到
            break  
        elif r_point1 == 126:      # 如果找到126都没找到  
            right_point_1 = 127    # 强制右左边点为127
            break  
    
    # 计算中点
    mid_point_1 = (left_point_1 + right_point_1) / 2  
    # 以这次中点作为下次的上次中点  
    last_mid_point_1 = int(mid_point_1)
    # 返回当前中点  
    return mid_point_1


left_point_2 = False
right_point_2 = False
last_mid_point_2 = 0  # 上次中点，应在函数外部初始化  
mid_point_2 = 60       # 当前中点，应在函数外部初始化
def get_ccd2_mid_point(bin_ccd):  
    global last_mid_point_2, mid_point_2, left_point_2, right_point_2, n, N,x,ccd2_left_idx_buxian,ccd2_right_idx_buxian
    
    # 搜索左边点，以上次中点作为这次的起搜点  
    for l_point2 in range(last_mid_point_2, 1, -1):  
        if bin_ccd[l_point2 - 1] == 0 and bin_ccd[l_point2] == 1:    # 判断是否与左边点一致
            left_point_2 = l_point2     # 左边点找到
            break  
        elif l_point2 == 1:     # 如果找到1都没找到
            left_point_2 = 0    # 强制令左边点为0  
            break  

      
    # 搜索右边点，以上次中点作为这次的起搜点  
    for r_point2 in range(last_mid_point_2, 126):  # 注意这里应该是128，因为索引是从0开始的  
        if bin_ccd[r_point2] == 1 and bin_ccd[r_point2 + 1] == 0:    # 判断是否与右边点一致  
            right_point_2 = r_point2    # 右边点找到  
            break  
        elif r_point2 == 126:      # 如果找到126都没找到    
            right_point_2 = 127    # 强制右左边点为127  
            break
    
    ccd2_left_idx_buxian = left_point_2
    ccd2_right_idx_buxian = right_point_2
    if(x == 1):
        ccd2_left_idx_buxian = right_point_2 - 88
    if(x == 2):
        ccd2_right_idx_buxian = left_point_2 + 10
        
    if(x == 3):
        ccd2_left_idx_buxian = right_point_2 - 90
    if(x == 4):
        ccd2_left_idx_buxian = right_point_2 - 90 
        
   
    # 计算中点
#     mid_point_2 = (left_point_2 + right_point_2) / 2
    mid_point_2 = (ccd2_left_idx_buxian + ccd2_right_idx_buxian) / 2  

    # 以这次中点作为下次的上次中点  
    last_mid_point_2 = int(mid_point_2)
    # 返回当前中点  
    return mid_point_2
    
    
    
def err_cal(ccd1_data,ccd2_data):
    global ccd1_center,ccd2_center,ccd1_err,ccd2_err
    thres1 = maxmin_threshold(ccd1_data)
    thres2 = maxmin_threshold(ccd2_data)
    
    image_value1 = ccd_image_value(ccd1_data, thres1)
    image_value2 = ccd_image_value(ccd2_data, thres2)
    
    ccd_filter(1),ccd_filter(2)
    
    Mid_point1 = get_ccd1_mid_point(image_value1)
    Mid_point2 = get_ccd2_mid_point(image_value2)
    
    ccd1_err = Mid_point1 - ccd1_mid
    ccd2_err = Mid_point2 - ccd2_mid    
    

    
    
    
    
beep    = Pin('C9' , Pin.OUT, value = False)
flag = 0
n = 0
N = 0
m = 0
M = 0
gyro_z1 = 0
angle_jizhun = 0
time_huandao = 0
flag_in_yuanhuan = 0

right_idx_history = [0,0,0,0]
left_idx_history = [0,0,0,0]
point_idx = 0



    
    
    
    
    
def add_idx(l,r):
    global right_idx_history,left_idx_history
    for i in range(1,4):
        right_idx_history[i-1] = right_idx_history[i]
        left_idx_history[i-1] = left_idx_history[i]
    right_idx_history[3] = r
    left_idx_history[3] = l
    

    
    
    

ccd1_center = 0
ccd1_center_last = 63
ccd1_left_idx = 0
ccd1_right_idx = 0
ccd1_left_idx_last = 0;
ccd1_right_idx_last = 0
ccd1_image = array('h',[0]*128)
ccd1_thres = 500
flag_ccd1_lose_left = False
flag_ccd1_lose_right = False
ac_ccd1_road_len = 60
ccd1_road_len = 55
ccd1_IllegalEdge = 3
ccd1_mid = 63

ccd2_center = 0
ccd2_center_last = 63
ccd2_left_idx = 0
ccd2_right_idx = 0
ccd2_left_idx_last = 0;
ccd2_right_idx_last = 0
ccd2_image = array('h',[0]*128)
ccd2_thres = 500
flag_ccd2_lose_left = False
flag_ccd2_lose_right = False
ac_ccd2_road_len = 60
ccd2_road_len = 75
ccd2_IllegalEdge = 3
ccd2_mid = 63


def erzhihua_xiajiangyan(ccd1_data,ccd2_data):
    '''
    二值化，检测下降沿
    
    参数：
        ccd1_data：ccd1图像数据 0 ~ 4095
        ccd2_data：ccd2图像数据 0 ~ 4095
        
    return：
        无    
    '''
    global ccd1_center
    global ccd1_center_last
    global ccd1_left_idx
    global ccd1_right_idx
    global ccd1_left_idx_last
    global ccd1_right_idx_last
    global ccd1_image
    global ccd1_thres
    global flag_ccd1_lose_left
    global flag_ccd1_lose_right
    global ac_ccd1_road_len
    global ccd1_road_len
    global ccd1_IllegalEdge
    global ccd1_mid
    
    global ccd2_center
    global ccd2_center_last
    global ccd2_left_idx
    global ccd2_right_idx
    global ccd2_left_idx_last
    global ccd2_right_idx_last
    global ccd2_image
    global ccd2_thres
    global flag_ccd2_lose_left
    global flag_ccd2_lose_right
    global ac_ccd2_road_len
    global ccd2_road_len
    global ccd2_IllegalEdge
    global ccd2_mid
    global ccd2_err
    global flag, n,m,N,M,right_idx_history,left_idx_history
    global x,ccd2_left_idx_buxian,ccd2_right_idx_buxian
    ccd1_ave_threshold = ave_threshold(ccd1_data)
    ccd2_ave_threshold = ave_threshold(ccd2_data)
    
######ccd1图像
#############################二值化
    
#     for i in range(2,125):
#         ccd1_image[i] = abs(ccd1_data[i-2] - ccd1_data[i+2]) // (ccd1_data[i-2]+ccd1_data[i+2]) * 1000
#         
#         if ccd1_data[i] < ccd1_thres :
#             ccd1_image[i] = 100
#         else:
#             ccd1_image[i] = 4000
    
    ccd1_left_idx_last = ccd1_left_idx
    ccd1_right_idx_last = ccd1_right_idx
    ccd1_center_last = ccd1_center
    ccd1_left_idx = ccd1_center_last
    ccd1_right_idx = ccd1_center_last
    
    while(ccd1_left_idx >= 0):
        if(ccd1_left_idx >= 4):
            if((ccd1_data[ccd1_left_idx] - ccd1_data[ccd1_left_idx - 4]) > ccd1_ave_threshold * 0.5):
                break;
        else:
            if(ccd1_data[ccd1_left_idx] < ccd1_ave_threshold):
                break
        ccd1_left_idx -= 1
        
    while(ccd1_right_idx <= 127):
        if(ccd1_right_idx <= 123):
            if((ccd1_data[ccd1_right_idx] - ccd1_data[ccd1_right_idx + 4]) >ccd1_ave_threshold * 0.5):
                break
        else:
            if(ccd1_data[ccd1_right_idx] < ccd1_ave_threshold):
                break
        ccd1_right_idx += 1
    '''
    #找边沿
    while(ccd1_image[ccd1_left_idx-1] >= ccd1_image[ccd1_left_idx]):
        ccd1_left_idx -= 1
    while(ccd1_image[ccd1_right_idx+1] >= ccd1_image[ccd1_right_idx]):
        ccd1_right_idx += 1
    '''
    if(ccd1_right_idx > 127):
        ccd1_right_idx = 127
    if(ccd1_left_idx < 0):
        ccd1_left_idx = 0
    
    add_idx(ccd1_left_idx,ccd1_right_idx)
    
    ac_ccd1_road_len = ccd1_right_idx - ccd1_left_idx #计算宽度偏差
    
    
    ##########二值化后的图像
#     for i in range(0,128):
#         if(i <= ccd1_right_idx and i > ccd1_left_idx):
#             ccd1_image[i] = 3000
#         else:
#             ccd1_image[i] = 1000
    
##############################丢边判断
    #两侧丢边 大幅度偏差
#     if(ac_ccd1_road_len - ccd1_road_len > 25):      
#         if (ccd1_left_idx <= 4*ccd1_IllegalEdge and ccd1_right_idx >= 127 -  4*ccd1_IllegalEdge ):
#             ccd1_center = ccd1_mid
#             flag_ccd1_lose_left = True
#             flag_ccd1_lose_right = True
#     
#     #两侧丢变 中等偏查
#     if(ac_ccd1_road_len - ccd1_road_len > 20):      
#         if (ccd1_left_idx <= 5*ccd1_IllegalEdge and ccd1_right_idx >= 127 -  5*ccd1_IllegalEdge ):
#             ccd1_center = ccd1_mid
#     #单侧丢边 小幅度偏差 
#     elif ac_ccd1_road_len - ccd1_road_len > 10:
#         
#         if (ccd1_left_idx <= 5*ccd1_IllegalEdge):
#             ccd1_center = ccd1_right_idx - ccd1_road_len//2
#             flag_ccd1_lose_left = True
#         elif (ccd1_right_idx >= 127 - 5*ccd1_IllegalEdge):
#             ccd1_center = ccd1_left_idx + ccd1_road_len //2
#             flag_ccd1_lose_right  = True
#     #正常情况
#     else:
    ccd1_center = (ccd1_left_idx + ccd1_right_idx) // 2
        
    
###########ccd2图像    
##############################二值化
    '''    
    for i in range(2,125):
        ccd2_image[i] = abs(ccd2_data[i-2] - ccd2_data[i+2]) // (ccd2_data[i-2]+ccd2_data[i+2]) * 1000
        
        if ccd2_data[i] < ccd2_thres :
            ccd2_image[i] = 100
        else:
            ccd2_image[i] = 4000
    '''        
    ccd2_left_idx_last = ccd2_left_idx
    ccd2_right_idx_last = ccd2_right_idx
    ccd2_center_last = ccd2_center
    ccd2_left_idx = ccd2_center_last
    ccd2_right_idx = ccd2_center_last
    
    while(ccd2_left_idx >= 0):
        if(ccd2_left_idx >= 4):
            if((ccd2_data[ccd2_left_idx] - ccd2_data[ccd2_left_idx - 4]) > ccd2_ave_threshold * 0.5):
                break;
        else:
            if(ccd2_data[ccd2_left_idx] < ccd2_ave_threshold):
                break
        ccd2_left_idx -= 1
    while(ccd2_right_idx <= 127):
        if(ccd2_right_idx <= 123):
            if((ccd2_data[ccd2_right_idx] - ccd2_data[ccd2_right_idx + 4]) >ccd2_ave_threshold * 0.5):
                break
        else:
            if(ccd2_data[ccd2_right_idx] < ccd2_ave_threshold):
                break
        ccd2_right_idx += 1
    '''
    #找边沿
    while(ccd2_image[ccd2_left_idx-1] >= ccd2_image[ccd2_left_idx]):
        ccd2_left_idx -= 1
    while(ccd2_image[ccd2_right_idx+1] >= ccd2_image[ccd2_right_idx]):
        ccd2_right_idx += 1
    '''
    ##########二值化后的图像
#     for i in range(0,128):
#         if(i <= ccd2_right_idx and i > ccd2_left_idx):
#             ccd2_image[i] = 3000
#         else:
#             ccd2_image[i] = 1000
    if(ccd2_right_idx > 127):
        ccd2_right_idx = 127
    if(ccd2_left_idx < 0):
        ccd2_left_idx = 0
    ac_ccd2_road_len = ccd2_right_idx - ccd2_left_idx #计算宽度偏差
    
    
##############################丢边判断
    #两侧丢边 大幅度偏差
    if(ac_ccd2_road_len - ccd2_road_len > 25):      
        if (ccd2_left_idx <= 4*ccd2_IllegalEdge and ccd2_right_idx >= 127 -  4*ccd2_IllegalEdge ):
#             ccd2_center = ccd2_mid
            flag_ccd2_lose_left = True
            flag_ccd2_lose_right = True
    
    #两侧丢变 中等偏查
#     if(ac_ccd2_road_len - ccd2_road_len > 20):      
#         if (ccd2_left_idx <= 5*ccd2_IllegalEdge and ccd2_right_idx >= 127 -  5*ccd2_IllegalEdge ):
#             ccd2_center = ccd2_mid
    #单侧丢边 小幅度偏差 
#     elif ac_ccd2_road_len - ccd2_road_len > 10:
#         
#         if (ccd2_left_idx <= 5*ccd2_IllegalEdge):
#             ccd2_center = ccd2_right_idx - ccd2_road_len//2
#             flag_ccd2_lose_left = True
#         elif (ccd2_right_idx >= 127 - 5*ccd2_IllegalEdge):
#             #ccd2_center = ccd2_left_idx + ccd2_road_len //2
#             flag_ccd2_lose_right  = True
    #正常情况
    
#     if (n == 2):
#         
#         ccd2_left_idx = ccd2_right_idx - 55
#     if (N == 2):
#         ccd2_right_idx = ccd2_left_idx + 55
#         
#     if (n == 4):
#         ccd2_left_idx = ccd2_right_idx - 34
#         
#   
#        
#         
#     # ccd2右圆环补线
#     if (m == 2):
#         ccd2_right_idx = ccd2_left_idx +70
#     if (M == 2):
#         ccd2_left_idx = ccd2_right_idx - 45
#     if (m == 4):
#         ccd2_right_idx = ccd2_left_idx +40
    #todo 圆环补线
    
    ccd2_left_idx_buxian = ccd2_left_idx
    ccd2_right_idx_buxian = ccd2_right_idx
    if(x == 1):
        ccd2_left_idx_buxian = ccd2_right_idx - 65
    if(x == 2):
        ccd2_right_idx_buxian = ccd2_left_idx + 60
        
    if(x == 3):
        ccd2_left_idx_buxian = ccd2_right_idx - 70
    if(x == 4):
        ccd2_left_idx_buxian = ccd2_right_idx - 70
    
    
    
    
    
    
    ccd2_center = (ccd2_left_idx_buxian + ccd2_right_idx_buxian) // 2
    #简单的两边丢线 十字处理 保持上次的占空比
    ccd2_err = ccd2_center - ccd2_mid
    if (ccd2_left_idx <= 10 and ccd2_right_idx >= 115 ):
        return 1
    else:
        return 0
#元素      
flag_annual = 0    #圆环
annual_delay = 0   #延时
annual_all_err1 = 0
annual_ave_err1 = 0
annual_all_err2 = 0
annual_ave_err2 = 0
annual_all_tips = 0
speed_type = 0
Stop = 0
annual_delay_time = 0
annual_state = 0
annual_dir = 0
annual_delay = 0
annual_yaw_angle = 0
flag_annual_detect = 0
allow_annual_time = 0

def go_annual():
    '''
    元素判断状态机,现主要判断圆环
    
    参数：
        ac_ccd1_road_len： ccd1实际道路长度
        ac_ccd2_road_len： ccd2实际道路长度
        ccd1_left_index： ccd1左边界
        ccd1_right_index： ccd1右边节
        ccd2_left_index： ccd2左边界
        ccd2_right_index： ccd2有边界
        annual_state： 环岛检测变量，用于构成状态机
        flag_annual_detect：
        annual_yaw_angle：
        annual_dir
    
    return：
    
    '''
    global ac_ccd1_road_len,ac_ccd2_road_len,ccd1_left_idx,ccd1_right_idx,ccd2_left_idx,ccd2_right_idx,annual_state,flag_annual_detect,annual_yaw_angle,annual_dir,yaw_angle
    global flag_annual
    global ccd1_road_len
    global ccd2_road_len
    global annual_delay
    global ccd1_mid
    global ccd2_mid
    global annual_all_err1
    global annual_ave_err1
    global annual_all_err2
    global annual_ave_error2
    global annual_all_tips
    global ccd1_IllegalEdge
    global ccd2_IllegalEdge
    global ccd1_center
    global ccd2_center
    global ccd1_right_idx_last
    global ccd1_left_idx_last
    global speed_type
    global Stop
    global annual_delay_time
    global angle_jizhun
    #左右圆环扣检测
    flag_annual_detect,annual_dir =  leftanulusdetect()
    
#     flag_annual_detect,annual_dir =  rightanulusdetect(ac_ccd1_road_len,
#                                                       ac_ccd2_road_len,
#                                                       ccd1_left_idx,
#                                                       ccd1_right_idx,
#                                                       ccd2_left_idx,
#                                                       ccd2_right_idx,
#                                                       ccd1_right_idx_last,
#                                                       annual_state,
#                                                       flag_annual_detect)
    # 0状态，正常寻迹，未检测到圆环入口
    if( annual_state == 0 and annual_dir >= 1 ):
        annual_state = 1
 
    
    # 1状态，已进入环岛，待转到一定角度后退出环岛  
#     if(annual_state == 1 and abs(annual_yaw_angle) <= 200):
#         return flag_annual,annual_state,annual_dir,flag_annual_detect,annual_yaw_angle
    
    # 检测到远端ccd丢线，进入出圆环状态
#     if(annual_state == 1):
#         if(abs(annual_yaw_angle) >= 300):
#             if(ccd1_right_index >= 127 - 2*ccd1_IllegalEdge and annual_dir == 1 ):
#                 flag_annual_detect = 0
#                 annual_state = 2
#                 
#             if(ccd1_left_index <= 2*ccd1_IllegalEdge and annual_dir == 2 ):
#                 flag_annual_detect = 0
#                 annual_state = 2
    if(annual_state == 1):
       
        if(ccd1_right_idx >= 127 - 2*ccd1_IllegalEdge and annual_dir == 1 ):
            flag_annual_detect = 0
            annual_state = 2
                
        if(ccd1_left_idx <= 2*ccd1_IllegalEdge and annual_dir == 2 ):
            flag_annual_detect = 0
            annual_state = 2
            
    # 当赛道长度恢复正常时进入状态3
    if(annual_state == 2 ):#按平均误差走
        
        if(ac_ccd1_road_len <=  75 ):#直到到达圆中
            annual_state = 3
            annual_all_error1 = 0
            annual_all_error2 = 0
            annual_all_tips = 0
            flag_annual_detect = 0
    
    #延时，防止再进入圆环       
    if(annual_state == 3):
        annual_delay += 1
        if(annual_delay >= annual_delay_time):
            annual_dir = 0
            annual_state = 0
            annual_yaw_angle = 0
            annual_delay = 0
            
    flag_annual = annual_state
    
    return flag_annual,annual_state,annual_dir,flag_annual_detect,annual_yaw_angle
        
#左圆环口识别

annual_debug = 0

def leftanulusdetect():
    global ac_ccd1_road_len
    global ac_ccd2_road_len,ccd1_left_idx,ccd1_right_idx,ccd2_left_idx,ccd2_right_idx,ccd1_left_idx_last,annual_state,flag_annual_detect
    global flag_annual
    global annual_dir
    global ccd1_road_len
    global ccd2_road_len
    global ccd1_mid
    global ccd2_mid
    global annual_all_err1
    global annual_ave_err1
    global annual_all_err2
    global annual_ave_err2
    global annual_all_tips
    global ccd1_IllegalEdge
    global ccd2_IllegalEdge
    global annual_debug
    global ccd1_center
    global ccd2_center
    global allow_annual_time
    
    ccd1_half_road_len = ccd1_road_len//2
    ccd2_half_road_len = ccd2_road_len//2
    
    #初始条件筛选 筛选出不做识别的部分
    # 已经在圆环中（annual_state > 0）
    # 赛道长度变长不大（<30）
    # 车头不正（即偏移赛道中心过大）
#     if(flag_annual_detect == 0):
#         if(annual_state > 0 or
#            ac_ccd1_road_len < ccd1_road_len + 20 
#            or abs(ccd2_center - ccd2_mid)>10 or
#            abs(ccd1_center - ccd1_mid)>10
#         ):
#             flag_annual_detect = 0
#             allow_annual_time = 0
#             return flag_annual_detect,annual_dir
#     
    #预判圆环
    # 宽度超过阈值且左侧丢线
    if(ac_ccd1_road_len > ccd1_road_len +  20 and
       annual_state == 0 and
       flag_annual_detect == 0
    ):
        if(ccd1_left_idx <= 15):
            flag_annual_detect = 1
        if(ccd1_right_idx >= 127 - 4*ccd1_IllegalEdge):
            flag_annual_detect = 0
            annual_dir = 0
            
            allow_annual_time = 0
    
    
    #保护
    if(flag_annual_detect > 4 or annual_state != 0 or flag_annual != 0):
        return flag_annual_detect,annual_dir
    
    
    if(flag_annual_detect == 1 and flag_annual == 0 ):
    #计时器，单位为帧    
#         allow_annual_time += 1
        
        #如果左边界开始向内缩短，说明存在入口转到状态2
        if(ccd1_left_idx - ccd1_left_idx_last >=1 and
           annual_state == 0 and ccd1_left_idx > 2*ccd1_IllegalEdge
        ):
            flag_annual_detect = 2
            allow_annual_time = 0
        
        # 如果此时右边线丢失，说明此时可能是两边丢线，并非圆环
        if(ccd1_right_idx >= 127 - 4*ccd1_IllegalEdge):
            flag_annual_detect = 0
            annual_dir = 0
            allow_annual_time =0
            
        #最多25帧，防止卡死
        if(allow_annual_time > 25):
            allow_annual_time = 0
            flag_annual_detect = 0
            
    if(flag_annual_detect == 2 and flag_annual == 0):
        #计时
#         allow_annual_time += 1
        
        
        #检测到左边界开始向左变长且近端赛道长度在阈值内，
        #即此时远端左边变长，近端检测到圆环的中间小圆，说明已经到达圆环入口
        if(ccd1_left_idx - ccd1_left_idx_last <= -1 and annual_state == 0):
            if(abs(ac_ccd2_road_len - ccd2_road_len)<15):
                flag_annual_detect = 3
                allow_annual_time = 0
        
        #保护，最多计时25帧        
        if(allow_annual_time > 25):
            allow_annual_time = 0
            flag_annual_detect = 0
    
    if(flag_annual_detect == 3):
#         allow_annual_time += 1
        
        #左边丢线，开始左转
        if(ccd2_left_idx <= 20):
            annual_dir = 1
        if(allow_annual_time > 25):
            allow_annual_time = 0
            flag_annual_detect = 0
        
    return flag_annual_detect,annual_dir
    
    
def rightanulusdetect(
    ac_ccd1_road_len,
    ac_ccd2_road_len,
    ccd1_left_idx,
    ccd1_right_idx,
    ccd2_left_idx,
    ccd2_right_idx,
    ccd1_right_idx_last,
    annual_state,
    flag_annual_detect
):
    global annual_flag
    global annual_dir
    global ccd1_road_len
    global ccd2_road_len
    global ccd1_mid
    global ccd2_mid
    global annual_all_err1
    global annual_ave_err1
    global annual_all_err2
    global annual_ave_err2
    global annual_all_tips
    global ccd1_IllegalEdge
    global ccd2_IllegalEdge
    global ccd1_center
    global ccd2_center
    global allow_annual_time
    
    ccd1_half_road_len = ccd1_road_len//2
    ccd2_half_road_len = ccd2_road_len//2
    
    if(flag_annual_detect == 0):
        if(annual_state > 0 or
           ac_ccd1_road_len < ccd1_road_len + 30 or
           abs(ccd2_center - ccd2_mid) > 10 or
           abs(ccd1_center - ccd1_mid) > 10
        ):
            flag_annual_detect = 0
            allow_annual_time = 0
            return flag_annual_detect ,annual_dir
    
    
    if(ac_ccd1_road_len > ccd1_road_len + 30 and
       annual_state == 0 and
       flag_annual_detect
    ):
        if(ccd1_right_idx >= 127 - 2*ccd1_IllegalEdge):
            flag_annual_detect = 5
            annual_dir = 0
            allow_annual_time =0
        if(ccd1_left_idx <= 4*ccd1_IllegalEdge):
            flag_annual_detect = 0
            annual_dir = 0
            allow_annual_time =0
    
    
    if(flag_annual_detect < 5 or
       annual_state != 0 or
       flag_annual != 0
    ):
        return flag_annual_detect,annual_dir
    
    
    if(flag_annual_detect == 5 and flag_annual == 0):
        allow_annual_time += 1
        
        if(ccd1_right_idx - ccd1_right_idx_last <= -1 and
           annual_state == 0 and
           ccd1_right_idx <127 - 2*ccd1_IllegalEdge
        ):
            flag_annual_detect == 6
            allow_annual_time = 0
            annual_dir = 0
        if(ccd1_left_idx <= 4*ccd1_IllegalEdge):
            flag_annual_detect = 0
            allow_annual_time = 0
            annual_dir = 0
        if(allow_annual_time > 25):
            allow_annual_time = 0
            flag_annual_detect = 0
            annual_dir =0
    
    if(flag_annual_detect == 6 and flag_annual == 0):
        allow_annual_time += 1
        if(ccd1_right_idx - ccd1_right_idx_last >= 1 and annual_state == 0):
            if(abs(ac_ccd2_road_len - ccd2_road_len) < 15):
                flag_annual_detect = 7
                allow_annual_time = 0
                annual_dir = 0
        if(allow_annual_time > 25):
            flag_annual_detect = 0
            allow_annual_time = 0
    
    
    if(flag_annual_detect == 7):
        allow_annual_time += 1
        
        if(allow_annual_time < 25 and  ccd1_right_idx >= 127 - 2*ccd1_IllegalEdge):
            annual_dir = 2
            
        if(allow_annual_time > 25):
            flag_annual_detect = 0
            allow_annual_time = 0
    return flag_annual_detect,annual_dir


ccd1_err = 0
ccd2_err = 0
ccd1_err_last = 0
ccd2_err_last = 0
all_road_len = 0
annual_out_offset = 2
parking_tip1 = 0 #停车控制，暂未加
parking_tip2 = 0
#方向控制
def dir_control(ccd1_left_idx,
                ccd1_right_idx,
                ccd2_left_idx,
                ccd2_right_idx,
                flag_annual_detect,
                flag_annual,
                annual_dir,
                annual_all_err1,
                annual_all_err2,
                annual_all_tips):
    '''
    方向控制
    
    参数：
    
    return：
    '''
    global ccd1_road_len
    global ccd2_road_len
    global ccd1_center
    global ccd2_center
    global ccd1_mid
    global ccd2_mid
    global ccd1_err
    global ccd2_err
    global ccd1_err_last
    global ccd2_err_last
    global ccd1_center_last
    global ccd2_center_last
    global ac_ccd1_road_len
    global ac_ccd2_road_len
    global ccd1_IllegalEdge
    global ccd2_IllegalEdge
    global all_road_len
    global annual_ave_err1
    global annual_ave_err2
    global annual_out_offset
    global parking_tip1
    global parking_tip2
    
    ccd1_center_last = ccd1_center
    ccd2_center_last = ccd2_center
    
    ccd1_err_last = ccd1_err
    ccd2_err_last = ccd2_err
    #初次检测环岛直行 
    if( flag_annual_detect == 2):
        ccd1_center = ccd1_right_idx - ac_ccd1_road_len//2
        ccd2_center = ccd2_right_idx - ac_ccd2_road_len//2
        
    if(flag_annual_detect == 3 or flag_annual):
        if(flag_annual_detect == 3 and annual_dir == 1):##坐
            ccd1_center = ccd1_left_idx + int(ccd1_road_len//annual_out_offset)
            ccd2_center = ccd2_left_idx + int(ccd2_road_len//annual_out_offset)
        elif(flag_annual_detect == 7 and annual_dir == 2):#右
            ccd1_center = ccd1_right_idx - int(ccd1_road_len//annual_out_offset)
            ccd2_center = ccd2_right_idx - int(ccd2_road_len//annual_out_offset)
        
        if(flag_annual == 3 and annual_dir == 1):#做
            ccd1_center = ccd1_right_idx - int(ccd1_road_len//annual_out_offset)
            ccd2_center = ccd2_right_idx - int(ccd2_road_len//annual_out_offset)
        elif(flag_annual == 3 and annual_dir == 2):##右
            ccd1_center = ccd1_left_idx + int(ccd1_road_len//annual_out_offset)
            ccd2_center = ccd2_left_idx + int(ccd2_road_len//annual_out_offset)
    
    if(ccd1_center >= 127):
        ccd1_center = 127
    elif(ccd1_center <= 0):
        ccd1_center = 0
    
    if(ccd2_center >= 127):
        ccd2_center = 127
    elif(ccd2_center <= 0):
        ccd2_center = 0   
    
    ccd1_err = ccd1_center - ccd1_mid
    ccd2_err = ccd2_center - ccd2_mid
    
    if(flag_annual == 1):
        annual_all_err1 = annual_all_err1 + ccd1_err
        annual_all_err2 = annual_all_err2 + ccd2_err
        annual_all_tips = annual_all_tips + 1
        annual_ave_err1 = int(annual_all_err1/annual_all_tips)
        annual_ave_err2 = int(annual_all_err2/annual_all_tips)
    if(flag_annual == 2 and annual_state == 2):
        if(annual_dir == 1):#做出圆环
            if(abs(ccd1_left_idx + ccd1_road_len//annual_out_offset - ccd1_mid) > abs(annual_ave_err1)):
                ccd1_err = annual_ave_err1
            else:
                ccd1_err = int((ccd1_left_idx + ccd1_road_len//annual_out_offset - ccd1_mid))
            
            if(abs(ccd2_left_idx + ccd2_road_len//annual_out_offset - ccd2_mid)>abs(annual_ave_err2)):
                ccd2_err = annual_ave_err2
            else:
                ccd2_err = int((ccd2_left_idx + ccd2_road_len//annual_out_offset - ccd2_mid))
        
        if(annual_dir == 2):#右出圆环
            if(abs(ccd1_right_idx - ccd1_road_len//annual_out_offset -ccd1_mid)>abs(annual_ave_err1)):
                ccd1_err = annual_ave_err1
            else:
                ccd1_err = int((ccd1_right_idx - ccd1_road_len//annual_out_offset -ccd1_mid))
            
            if(abs(ccd2_right_idx - ccd2_road_len//annual_out_offset -ccd2_mid)>abs(annual_ave_err2)):
                ccd1_err = annual_ave_err1
            else:
                ccd2_err = int((ccd2_right_idx - ccd2_road_len//annual_out_offset -ccd2_mid))


counter_element = 0
def get_ccd_err(ccd1_data,ccd2_data,imu_data,encl,encr):
    global ccd1_err
    global ccd2_err
    global counter_element
    
#     counter_element += 10
#     
#     
#     if(counter_element > 100000000):
#         counter_element = 1010
#     
#     if(counter_element > 1000):
# #         dir_control(ccd1_left_idx,ccd1_right_idx,ccd2_left_idx,ccd2_right_idx,flag_annual_detect,flag_annual,annual_dir,annual_all_err1,annual_all_err2,annual_all_tips)
# #         go_annual()
# #         element()
# #         process_element(imu_data,encl,encr)
#         my_yuanhuan_detect()
#         my_huanhuan_yunxing(imu_data)
    
#     cross_flag = erzhihua_xiajiangyan(ccd1_data,ccd2_data)
    
    err_cal(ccd1_data,ccd2_data)

    ccd_err =  ccd2_err 
    
    return ccd_err,0 


def qushi():
    global right_idx_history,left_idx_history
    flag_r_turan_diuxian = 0
    flag_l_turan_diuxian = 0
    
    if(abs(right_idx_history[3] - right_idx_history[0]) > 25):
        flag_r_turan_diuxian = 1    
    if(abs(left_idx_history[3] - left_idx_history[0]) > 25 ):
        flag_l_turan_diuxian = 1
    
    return flag_l_turan_diuxian,flag_r_turan_diuxian


def element():
    global ccd1_left_idx,ccd1_right_idx,flag,ccd2_left_idx,ccd2_right_idx,flag_in_yuanhuan,right_idx_history,left_idx_history
    

    if(flag_in_yuanhuan == 0):
        
        
        if(ccd2_left_idx > 10 and ccd2_right_idx < 118):
            if(ccd1_left_idx < 15 and  70 < ccd1_right_idx < 95 ):
                l,r = qushi()
                if(l == 1):
                    if(abs(ccd1_right_idx - ccd2_right_idx) <= 16):
                        flag = 1
    
        if(abs(ccd1_right_idx - ccd2_right_idx) <= 20 and ccd2_left_idx < 15 and flag == 1) :
            flag = 2
        
#         if(ccd2_left_idx > 10 and ccd2_right_idx < 118 ):
#             if(ccd1_right_idx > 105 and ccd1_left_idx > 10):
#                 l,r = qushi()
#                 if(r == 1):
#                     if(abs(ccd1_left_idx - ccd2_left_idx) <= 20):
#                         flag = 3
#     
#         if(abs(ccd1_left_idx - ccd2_left_idx) <= 20 and ccd2_right_idx > 105 and ccd2_left_idx > 15 and flag == 3 ):
#             flag = 4
    
distance_T = 0

def process_element(imu_data,encl_data,encr_data):
    global flag,n,N,m,M ,gyro_z1,time_huandao,flag_in_yuanhuan,distance_T
    if(flag == 1 and n == 0):
        n = 1
    if(flag == 2 and n == 1):
        n = 2
        N = 1
        time_huandao = 0
        distance_T = 0
    if(n == 2):
        flag_in_yuanhuan = 1
        gyro_z1 += imu_data[5] *0.07 *0.01
        distance_T += (encl_data + encr_data) / 2 / 1024 * 30 / 50 * 0.01 * 3.1415926
        time_huandao += 10
#         beep.high()
        if( -0.8 < gyro_z1 < 0.8 ):        #防误判弯道
            if (distance_T <-0.07):
                beep.low()
                gyro_z1 = 0
                n = 3
                N = 2  #环补线
                time_huandao = 0
                distance_T = 0
            if(-0.8> gyro_z1 ) or ( 0.8 < gyro_z1):
                n = 0
                N = 0
                gyro_z1= 0
                time_huandao = 0
                distance_T = 0
    if(N == 2):
        
        gyro_z1 += imu_data[5] *0.07 *0.01
        
        if(gyro_z1 > 240):
            N = 3
            n = 4
            
    if(n == 4):
        gyro_z1 = 0
        distance_T += (encl_data + encr_data) / 2 / 1024 * 30 / 50 * 0.01 * 3.1415926
        time_huandao+=10
#         beep.high()
        if(distance_T  <-0.1):
            beep.low()
            n = 0
            N = 0
            time_huandao = 0
            flag_in_yuanhuan = 0
    
#     if(flag == 3 and m == 0):
#         m = 1
#     if(flag == 4 and m ==1):
#         m = 2
#         M = 1
#         distance_T = 0
#     if(m == 2):
#         gyro_z1 += imu_data[5] *0.07 *0.01
#         flag_in_yuanhuan = 1
#         distance_T += (encl_data + encr_data) / 2 / 1024 * 30 / 50 * 0.01 * 3.1415926
# #         beep.high()
#         if(-0.8 < gyro_z1 < 0.8):
#             if(distance_T < -0.08):
#                 gyro_z1 = 0
#                 flag = 0
#                 m = 3
#                 M = 2
#                 distance_T = 0
#                 beep.low()
#         if(gyro_z1 > 0.8 or gyro_z1 < -0.8):
#             m = 0
#             M = 0
#             gyro_z1 = 0
#     if(M == 2):
#         gyro_z1 += imu_data[5] *0.07 *0.01
#         if(gyro_z1 < -240):
#             M = 3
#             m = 4
# #             beep.high()
#     if(m == 4):
#         gyro_z1 = 0
#         distance_T += (encl_data + encr_data) / 2 / 1024 * 30 / 50 * 0.01 * 3.1415926
#         if(distance_T < -0.08):
#             m = 0
#             M = 0
#             distance_T = 0
#             gyro_z1 = 0
#             beep.low()




huandaogeshu = 0


# def my_yuanhuan_detect():
#     global ccd1_left_idx,ccd1_right_idx,ccd2_left_idx,ccd2_right_idx,fflag,x
#     
#     
#     if(ccd1_left_idx < 16 and ccd2_left_idx < 12 and  60 < ccd1_right_idx < 115 and ccd2_right_idx < 115 and fflag == 0 ):
#         fflag = 1
#     
#     if( ccd1_left_idx<16 and 15  < ccd2_left_idx and   ccd1_right_idx < 115 and ccd2_right_idx < 115 and fflag == 1 ):
#         fflag = 2
#         if(ccd1_right_idx<75):
#             fflag = 0
#     if((ccd1_left_idx < 12 and 115 < ccd1_right_idx ) or (ccd2_left_idx < 12 and 115 < ccd2_right_idx)):
#         fflag = 0
#         
# #     if(20 < ccd1_left_idx and 20 < ccd2_left_idx and ccd1_right_idx < 120 and ccd2_right_idx < 120):
# #             fflag = 0
# def my_huanhuan_yunxing(imu_data):
#     global huandaogeshu
#     global ccd1_left_idx,ccd1_right_idx,ccd2_left_idx,ccd2_right_idx,fflag,x,gyro,dist,lose_r_flag,beep
#     
#     
#     #检测到单边丢线，准备沿右边界补线行驶至两边正常
#     if(huandaogeshu == 0 and x == 0 and fflag == 1):
#         huandaogeshu = 1
#         x = 1
#     #检测到两边正常，准备沿左边界补线至两侧再次正常````````````````
#     if(x == 1 and fflag == 2):
#         x = 2
#         gyro = 0
#         
#     #沿左边界走，对z轴角速度进行积分，防止提早判断出环
#     if(x == 2):
#        
#         
#         if( 115 < ccd2_right_idx and lose_r_flag == 0):
#                lose_r_flag += 1
#             
#         gyro += imu_data[5]*0.0007
#         #在至少转过100度后才进行边界判断
#         if(gyro > 100):
#             
#             #两边界恢复正常，开始准备沿右边界补线
#             if(lose_r_flag == 1 and 115 < ccd2_right_idx ):
#                 lose_r_flag += 1
#             if(6 < ccd2_left_idx and ccd2_right_idx < 115 and lose_r_flag >= 2):
#                 x = 3
#                 gyro = 0
#                 lose_r_flag = 0
#                 beep.high()
#     #沿右边界补线，直到检测到左边再次丢线    
#     if(x == 3):
#         
#         if(ccd1_left_idx < 12 and ccd2_left_idx < 12 and ccd1_right_idx < 120 and ccd2_right_idx < 120):
#             x = 4
#             beep.low()
#     #继续沿右边界补线，直到赛道正常
#     if(x == 4 ):
#         if(20 < ccd1_left_idx and 20 < ccd2_left_idx and ccd1_right_idx < 120 and ccd2_right_idx < 120):
#             x = 0
#             fflag = 0
        
        
        

def my_yuanhuan_detect():
    global left_point_1,right_point_1,left_point_2,right_point_2,fflag,x
    
    
    if(left_point_1 < 16 and left_point_2 < 12 and  80 < right_point_1 < 115 and right_point_2 < 115 and fflag == 0 ):
        fflag = 1
    
    if(fflag == 1):
        if(right_point_1 < 70 or right_point_2 < 70):
            fflag = 0 
    
    if( left_point_1<16 and 15  < left_point_2 and   right_point_1 < 115 and right_point_2 < 115 and fflag == 1 ):
        
        
        fflag = 2
        if(right_point_1<75):
            fflag = 0
    if((left_point_1 < 12 and 115 < right_point_1 ) or (left_point_2 < 12 and 115 < right_point_2)):
        fflag = 0
        
#     if(20 < left_point_1 and 20 < left_point_2 and right_point_1 < 120 and right_point_2 < 120):
#             fflag = 0
def my_huanhuan_yunxing(imu_data):
    global huandaogeshu
    global left_point_1,right_point_1,left_point_2,right_point_2,fflag,x,gyro,dist,lose_r_flag,beep
    
    if(fflag == 0):
        x = 0
    #检测到单边丢线，准备沿右边界补线行驶至两边正常
    if(huandaogeshu == 0 and x == 0 and fflag == 1):
        huandaogeshu = 1
        x = 1
    #检测到两边正常，准备沿左边界补线至两侧再次正常````````````````
    if(x == 1 and fflag == 2):
        x = 2
        gyro = 0
        
    #沿左边界走，对z轴角速度进行积分，防止提早判断出环
    if(x == 2):
       
        
        if( 115 < right_point_2 and lose_r_flag == 0):
               lose_r_flag += 1
            
        gyro += imu_data[5]*0.0007
        #在至少转过100度后才进行边界判断
        if(gyro > 100):
            
           
            if(lose_r_flag == 1 and 115 < right_point_2 ):
                lose_r_flag += 1
#             if(6 < left_point_2 and right_point_2 < 115 and lose_r_flag >= 2):
            if(right_point_2 < 100 and lose_r_flag >= 2):
                x = 3
                gyro = 0
                lose_r_flag = 0
                beep.high()
    #沿右边界补线，直到检测到左边再次丢线    
    if(x == 3):
        
        if(left_point_1 < 16 and left_point_2 < 16 and right_point_1 < 120 and right_point_2 < 120):
            x = 4
            beep.low()
    #继续沿右边界补线，直到赛道正常
    if(x == 4 ):
        if(20 < left_point_1 and 20 < left_point_2 and right_point_1 < 120 and right_point_2 < 120):
            x = 0
            fflag = 0
    
    
    
    
